#include <algorithm/analytical_model/algorithm/cpu/receiver/CylinderReceiver.h>
#include <algorithm/analytical_model/algorithm/cuda/VectorMath.cuh>
#include <algorithm/analytical_model/algorithm/cuda/inter_op/Data.h>
#include <algorithm/analytical_model/algorithm/cuda/receiver/CylinderReceiver.cuh>
#include <algorithm/analytical_model/algorithm/cuda/receiver/Receiver.cuh>
#include <utils/Utils.h>


#include <cmath>

using namespace solar::cuda;
using namespace solar::cuda::inter_op;

CylinderReceiver::CylinderReceiver(cudaStream_t stream) : Receiver(stream) {}

void CylinderReceiver::init(float pixel_per_meter_for_receiver)
{
    pixel_length_ = 1.0F / float(pixel_per_meter_for_receiver);
    setResolution(pixel_per_meter_for_receiver);
    allocImage();
    cleanImageContent();
}

void CylinderReceiver::setResolution(float pixel_per_meter_for_receiver)
{
    resolution_.x =
        int(std::ceil(2 * inter_op::kMathPi * size_.x * float(pixel_per_meter_for_receiver)));
    resolution_.y = int(size_.y * float(pixel_per_meter_for_receiver));
    n_resolution_ = resolution_.x * resolution_.y;
}

__host__ __device__ auto CylinderReceiver::getFocusCenter(const float3& heliostat_position) -> float3
{
    if (isInsideCylinder(heliostat_position))
    {
        // If the heliostat position is inside cylinder, return the receiver position
        return pos_;
    }
#if defined(__CUDA_ARCH__)
    float3 dir = normalize(heliostat_position - pos_);
    float radius = size_.x / std::hypotf(dir.x, dir.z);

    float x = pos_.x + dir.x * radius;
    float z = pos_.z + dir.z * radius;

    return ::make_float3(x, pos_.y, z);
#else
    auto result = solar::cpu::getFocusCenter(structCopy<Float3>(heliostat_position),
                                             structCopy<Float3>(size_), structCopy<Float3>(pos_));
    return structCopy<float3>(result);
#endif
}

__device__ __host__ auto CylinderReceiver::isInsideCylinder(const float3& orig) -> bool
{
#if defined(__CUDA_ARCH__)
    float2 l = ::make_float2(orig.x - pos_.x, orig.z - pos_.z);
    return dot(l, l) <= size_.x * size_.x;
#else
    return solar::cpu::isInsideCylinder(structCopy<Float3>(orig), structCopy<Float3>(size_),
                                        structCopy<Float3>(pos_));
#endif
}

__device__ __host__ auto CylinderReceiver::intersect(const float3& orig, const float3& dir)
    -> std::tuple<float, float, float, bool>
{
    float t = 0;
    float u = 0;
    float v = 0;
    // If the origin is inside the cylinder, it won't intersect with it
    if (isInsideCylinder(orig))
    {
        // printf("The origin is inside the cylinder!!!\n");
        return {t, u, v, false};
    }

    // simplified expression and got : Ro^2 - tp^2 <= R2, represent delta >= 0
    float2 Ro = ::make_float2(pos_.x - orig.x, pos_.z - orig.z);
    float tp = dot(Ro, normalize(::make_float2(dir.x, dir.z)));
    float delta = dot(Ro, Ro) - tp * tp;

    // Return false if:
    //  1) The direction is different
    //  2) Has no intersection
    float R2 = size_.x * size_.x; // radius ^ 2
    if (tp < -kEpsilon || delta > R2)
    {
        return {t, u, v, false};
    }
    // remain and calculate the positive solution
    float t_plus = delta <= 0.0F ? size_.x : sqrt(R2 - delta);
    t = (tp - t_plus) / std::hypotf(dir.x, dir.z);

    // calculate intersection point
    float3 intersect_pos = t * dir + orig;
    // printf( "intersect_pos: ( %f %f %f \n" ,intersect_pos.x,intersect_pos.y ,intersect_pos.z);
    u = (intersect_pos.y - pos_.y) / size_.y + 0.5F;
    // std::cout << "u = " << u << "\n";
    //  intersect_pos.y should belong to the range [0, h], h = size_.y
    if (u < 0.0F || u > 1.0F)
    {
        return {t, u, v, false};
    }

    float2 intersect_origin_dir = ::make_float2(intersect_pos.x - pos_.x, intersect_pos.z - pos_.z);
    intersect_origin_dir = normalize(intersect_origin_dir); // (cosine, sine)

    /**
     * TODO:ADD ERROR TESTS
     */
    if (intersect_origin_dir.x < -1 || intersect_origin_dir.x > 1)
    {
        // printf("\nError occurs on intersect position: %f, %f, %f\n", intersect_pos.x,
        // intersect_pos.y, intersect_pos.z);
    }

    v = std::acos(intersect_origin_dir.x) / (2 * inter_op::kMathPi);
    // std::cout << "v1 = " << v << "\n";
    if (intersect_origin_dir.y < 0)
    {
        v = 1 - v;
    }
    // std::cout << "v2 = " << v << "\n";
    return {t, u, v, true};
}
